import seltar.motion.*;

Motion m[];
float Angle = random(TWO_PI), Strokeweight = 1;
int arraySize = 20;
int numOfLines = 1;
static final float epsilon = 0.0001;
float arrx[][];
float arry[][];
int head[], tail[];
int heading[];

Point [][]routes;
int numOfDots = 300;

void generateRoutes()
{
  routes = new Point[numOfLines][];
  for (int i=0; i<numOfLines; ++i) {
    routes[i] = new Point[numOfDots];
    generateRoute(i);
  }
}

void generateRoute(int id)
{
  float x = random(0, width); 
  float y = random(0, height);
  //println("width: " + width + " height: " + height);
  for (int i=0; i<numOfDots; ++i) {
    routes[id][i] = new Point(x, y);
    x += random(-20, 20);
    if (x < 0) x = -x;
    if (x > width) x = width - (x - width);
    y += random(-20, 20);
    if (y < 0) y = -y;
    if (y > height) y = height - (y - height);
  }
}

void resetHeadings()
{
  for (int i=0; i<numOfLines; ++i) {
    heading[i] = 1;
  }
}

void initRoutes()
{
  generateRoutes();
  heading = new int[numOfLines];
  resetHeadings();
}

void setup()
{
  size(800,600);
  initRoutes();
  //frameRate(24);
  m = new Motion[numOfLines];
  for (int i=0; i<numOfLines; ++i) 
    m[i] = new Motion(routes[i][0].getX(), routes[i][0].getY());
  smooth();
  background(0);  
  stroke(200);
  arrx = new float[numOfLines][arraySize];
  arry = new float[numOfLines][arraySize];
  head = new int[numOfLines];
  tail = new int[numOfLines];
  for (int i=0; i<numOfLines; ++i) {
    head[i] = tail[i] = 0;
    arrx[i][head[i]] = routes[i][0].getX();
    arry[i][head[i]] = routes[i][0].getY();
  }
}

void resetArray(int id)
{
  head[id] = tail[id] = 0;
  arrx[id][head[id]] = routes[id][0].getX();
  arry[id][head[id]] = routes[id][0].getY();
}

float getDistance(Point pt1, Point pt2)
{
  float x1 = pt1.getX();
  float y1 = pt1.getY();
  float x2 = pt2.getX();
  float y2 = pt2.getY();
  return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

boolean isOnline(Point pt, Point startPos, Point endPos)
{
  float dis1 = getDistance(pt, startPos);
  float dis2 = getDistance(pt, endPos);
  float disa = getDistance(startPos, endPos);
  //println("dis1 " + dis1 + " dis2 " + dis2 + " disa " + disa);
  if (abs(dis1 + dis2 - disa) < epsilon) return true;
  else return false;
}

boolean isNear(int id)
{
  return !isOnline(m[id].pos, 
                   routes[id][heading[id]-1], 
                   routes[id][heading[id]-1]);
}

void printRoute(int id)
{
  for (int i=1; i<numOfDots; ++i) {
    line(routes[id][i-1].getX(), routes[id][i-1].getY(),
         routes[id][i].getX(), routes[id][i].getY());
  }
}

void printRoutes()
{
  for (int i=0; i<numOfLines; ++i) {
    printRoute(i);
  }
}

void resetMotion(int id)
{
  m[id].setX(routes[id][0].getX());
  m[id].setY(routes[id][0].getY());
  head[id] = tail[id] = 0;
}

void draw()
{
  background(0);
  //printRoutes();
  //if(mousePressed) background(0);
  for (int i=0; i<numOfLines; ++i) {
    float curx = m[i].getX();
    float cury = m[i].getY();
    if (isNear(i)) 
      ++heading[i];
    if (heading[i] == numOfDots) {
      heading[i] = 1;
      resetArray(i);
      resetMotion(i);
    } else {
        float angle = m[i].calcAngle(m[i].getX(), m[i].getY(), 
                                     routes[i][heading[i]].getX(),
                                     routes[i][heading[i]].getY());
        m[i].moveDir(angle, 1);
        m[i].move();
  //m.constrain(width/2, height/2,width/2,height/2);
  //strokeWeight(abs(Strokeweight));  
  // Draw
        addNewSection(i, curx, cury);
        drawLine(i);
    }
  }
}

void drawLine(int id)
{
  for (int i=tail[id]; i!=head[id]; i=(i+1)%arraySize) {
    int i1 = (i + 1) % arraySize;
    line(arrx[id][i], arry[id][i], arrx[id][i1], arry[id][i1]);
    //println("line " + i + " x1: " + arrx[id][i] + " y1: " + arry[id][i] + 
    //        " x2: " + arrx[id][i1] + " y2: " + arry[id][i1]); 
  }
}

void addNewSection(int id, float x, float y)
{
  head[id] = (head[id] + 1) % arraySize;
  if (head[id] == tail[id]) 
    tail[id] = (tail[id] + 1) % arraySize;
  arrx[id][head[id]] = x;
  arry[id][head[id]] = y;
  //for (int i=tail[id]; i!=head[id]; i=(i+1)%arraySize) {
  //  println("i: " + i + " X: " + arrx[id][i] + " Y: " + arry[id][i]);
  //}
}
